///////////////////////////////////////////////////////////////////////////////
//-----------------------------------------------------------------------------
#include <Math/Matrix4x4.h>
#include <Math/Vec3f.h>
#include <cmath>
#include <string.h>

//-----------------------------------------------------------------------------

Matrix4x4::Matrix4x4()
{
	SetIdentity();
}

//-----------------------------------------------------------------------------

Matrix4x4::~Matrix4x4()
{
}

//-----------------------------------------------------------------------------

void		Matrix4x4::Scale(f32 sx, f32 sy, f32 sz)
{
    m[0][0] *= sx;
    m[0][1] *= sx;
    m[0][2] *= sx;
    m[0][3] *= sx;

    m[1][0] *= sy;
    m[1][1] *= sy;
    m[1][2] *= sy;
    m[1][3] *= sy;

    m[2][0] *= sz;
    m[2][1] *= sz;
    m[2][2] *= sz;
    m[2][3] *= sz;
}

//-----------------------------------------------------------------------------

void		Matrix4x4::Translate(f32 tx, f32 ty, f32 tz)
{
    m[3][0] += (m[0][0] * tx + m[1][0] * ty + m[2][0] * tz);
    m[3][1] += (m[0][1] * tx + m[1][1] * ty + m[2][1] * tz);
    m[3][2] += (m[0][2] * tx + m[1][2] * ty + m[2][2] * tz);
    m[3][3] += (m[0][3] * tx + m[1][3] * ty + m[2][3] * tz);
}

//-----------------------------------------------------------------------------

void		Matrix4x4::Rotate(f32 angle, f32 x, f32 y, f32 z)
{
   f32 sinAngle, cosAngle;
   f32 mag = sqrtf(x * x + y * y + z * z);
      
   sinAngle = sinf ( angle * MGL_PI / 180.0f );
   cosAngle = cosf ( angle * MGL_PI / 180.0f );
   if ( mag > 0.0f )
   {
      f32 xx, yy, zz, xy, yz, zx, xs, ys, zs;
      f32 oneMinusCos;
      Matrix4x4 rotMat;
   
      x /= mag;
      y /= mag;
      z /= mag;

      xx = x * x;
      yy = y * y;
      zz = z * z;
      xy = x * y;
      yz = y * z;
      zx = z * x;
      xs = x * sinAngle;
      ys = y * sinAngle;
      zs = z * sinAngle;
      oneMinusCos = 1.0f - cosAngle;

      rotMat.m[0][0] = (oneMinusCos * xx) + cosAngle;
      rotMat.m[0][1] = (oneMinusCos * xy) - zs;
      rotMat.m[0][2] = (oneMinusCos * zx) + ys;
      rotMat.m[0][3] = 0.0F; 

      rotMat.m[1][0] = (oneMinusCos * xy) + zs;
      rotMat.m[1][1] = (oneMinusCos * yy) + cosAngle;
      rotMat.m[1][2] = (oneMinusCos * yz) - xs;
      rotMat.m[1][3] = 0.0F;

      rotMat.m[2][0] = (oneMinusCos * zx) - ys;
      rotMat.m[2][1] = (oneMinusCos * yz) + xs;
      rotMat.m[2][2] = (oneMinusCos * zz) + cosAngle;
      rotMat.m[2][3] = 0.0F; 

      rotMat.m[3][0] = 0.0F;
      rotMat.m[3][1] = 0.0F;
      rotMat.m[3][2] = 0.0F;
      rotMat.m[3][3] = 1.0F;

	  *this *= rotMat;
   }
}
/*
void anglesToAxes(const Vector3 angles, Vector3& left, Vector3& up, Vector3& forward)
{
    const float DEG2RAD = 3.141593f / 180;
    float sx, sy, sz, cx, cy, cz, theta;

    // rotation angle about X-axis (pitch)
    theta = angles.x * DEG2RAD;
    sx = sinf(theta);
    cx = cosf(theta);

    // rotation angle about Y-axis (yaw)
    theta = angles.y * DEG2RAD;
    sy = sinf(theta);
    cy = cosf(theta);

    // rotation angle about Z-axis (roll)
    theta = angles.z * DEG2RAD;
    sz = sinf(theta);
    cz = cosf(theta);

    // determine left axis
    left.x = cy*cz;
    left.y = sx*sy*cz + cx*sz;
    left.z = -cx*sy*cz + sx*sz;

    // determine up axis
    up.x = -cy*sz;
    up.y = -sx*sy*sz + cx*cz;
    up.z = cx*sy*sz + sx*cz;

    // determine forward axis
    forward.x = sy;
    forward.y = -sx*cy;
    forward.z = cx*cy;
}
*/

//-----------------------------------------------------------------------------

Matrix4x4		Matrix4x4::GetTranspose()
{
	Matrix4x4 tmp;
	for( u8 x = 0; x < 4; x++ )
		for( u8 y = 0; y < 4; y++ )
		{
			tmp.m[x][y] = m[y][x];
		}
	return tmp;
}

//-----------------------------------------------------------------------------

void		Matrix4x4::Transpose()
{
	*this = GetTranspose();
}

//-----------------------------------------------------------------------------

Matrix4x4		Matrix4x4::GetInverse()
{
	Matrix4x4 tmp;

	f64 determinant =
		+m[0][0]*(m[1][1]*m[2][2] - m[2][1]*m[1][2])
		-m[0][1]*(m[1][0]*m[2][2] - m[1][2]*m[2][0])
		+m[0][2]*(m[1][0]*m[2][1] - m[1][1]*m[2][0]);

	if( determinant < 0.001f || determinant > 1000 )
	{
		return tmp;
	}

	f64 invdet = 1.0f / determinant;

	tmp.m[0][0] =  (m[1][1]*m[2][2] - m[2][1]*m[1][2]) *invdet;
	tmp.m[1][0] = -(m[0][1]*m[2][2] - m[0][2]*m[2][1]) *invdet;
	tmp.m[2][0] =  (m[0][1]*m[1][2] - m[0][2]*m[1][1]) *invdet;
	tmp.m[0][1] = -(m[1][0]*m[2][2] - m[1][2]*m[2][0]) *invdet;
	tmp.m[1][1] =  (m[0][0]*m[2][2] - m[0][2]*m[2][0]) *invdet;
	tmp.m[2][1] = -(m[0][0]*m[1][2] - m[1][0]*m[0][2]) *invdet;
	tmp.m[0][2] =  (m[1][0]*m[2][1] - m[2][0]*m[1][1]) *invdet;
	tmp.m[1][2] = -(m[0][0]*m[2][1] - m[2][0]*m[0][1]) *invdet;
	tmp.m[2][2] =  (m[0][0]*m[1][1] - m[1][0]*m[0][1]) *invdet;

	return tmp;
}

//-----------------------------------------------------------------------------

void		Matrix4x4::Inverse()
{
	*this = GetInverse();
}

//-----------------------------------------------------------------------------

void		Matrix4x4::SetFrustum(f32 left, f32 right, f32 bottom, f32 top, f32 nearZ, f32 farZ)
{
    f32       deltaX = right - left;
    f32       deltaY = top - bottom;
    f32       deltaZ = farZ - nearZ;

    if ( (nearZ <= 0.0f) || (farZ <= 0.0f) ||
         (deltaX <= 0.0f) || (deltaY <= 0.0f) || (deltaZ <= 0.0f) )
         return;

    m[0][0] = 2.0f * nearZ / deltaX;
    m[0][1] = m[0][2] = m[0][3] = 0.0f;

    m[1][1] = 2.0f * nearZ / deltaY;
    m[1][0] = m[1][2] = m[1][3] = 0.0f;

    m[2][0] = (right + left) / deltaX;
    m[2][1] = (top + bottom) / deltaY;
    m[2][2] = -(nearZ + farZ) / deltaZ;
    m[2][3] = -1.0f;

    m[3][2] = -2.0f * nearZ * farZ / deltaZ;
    m[3][0] = m[3][1] = m[3][3] = 0.0f;
}

//-----------------------------------------------------------------------------

void		Matrix4x4::SetPerspective(f32 fovy, f32 aspect, f32 nearZ, f32 farZ)
{
   f32 frustumW, frustumH;
   
   frustumH = tanf( fovy / 360.0f * MGL_PI ) * nearZ;
   frustumW = frustumH * aspect;

   SetFrustum( -frustumW, frustumW, -frustumH, frustumH, nearZ, farZ );
}

//-----------------------------------------------------------------------------

void		Matrix4x4::SetOrtho(f32 left, f32 right, f32 bottom, f32 top, f32 nearZ, f32 farZ)
{
    f32       deltaX = right - left;
    f32       deltaY = top - bottom;
    f32       deltaZ = farZ - nearZ;

    if ( (deltaX == 0.0f) || (deltaY == 0.0f) || (deltaZ == 0.0f) )
        return;

    SetIdentity();
    m[0][0] = 2.0f / deltaX;
    m[3][0] = -(right + left) / deltaX;
    m[1][1] = 2.0f / deltaY;
    m[3][1] = -(top + bottom) / deltaY;
    m[2][2] = -2.0f / deltaZ;
    m[3][2] = -(nearZ + farZ) / deltaZ;
}

//-----------------------------------------------------------------------------

void		Matrix4x4::SetIdentity()
{
    memset( m, 0x0, sizeof( m ) );
    m[0][0] = 1.0f;
    m[1][1] = 1.0f;
    m[2][2] = 1.0f;
    m[3][3] = 1.0f;
}

//-----------------------------------------------------------------------------

bool		Matrix4x4::IsIdentity()
{
	for( u8 x = 0; x < 4; x++ )
	{
		for( u8 y = 0; y < 4; y++ )
		{
			if( !( ((m[x][y] == 1.0f) && (x == y)) || ((m[x][y] == 0.0f) && (x != y)) ) )
			{
				return false;
			}
		}
	}
	return true;
}

//-----------------------------------------------------------------------------

Vec3f		Matrix4x4::GetPosition()
{
	return Vec3f( m[3][0], m[3][1], m[3][2] );
}

//-----------------------------------------------------------------------------

void		Matrix4x4::GetPosition(Vec3f& pos)
{
	pos.set( m[3][0], m[3][1], m[3][2] );
}

//-----------------------------------------------------------------------------

void		Matrix4x4::SetPosition(f32 x, f32 y, f32 z, f32 w)
{
	m[3][0] = x;
	m[3][1] = y;
	m[3][2] = z;
	m[3][3] = w;
}

//-----------------------------------------------------------------------------

void		Matrix4x4::SetPosition(Vec3f& pos)
{
	SetPosition( pos.x, pos.y, pos.z );
}

//-----------------------------------------------------------------------------

Matrix4x4&	Matrix4x4::operator = (const Matrix4x4& mat)
{
	if( this != &mat )
	{
		memcpy( m, mat.m, sizeof( f32 ) * 16 );
	}
	return *this;
}

//-----------------------------------------------------------------------------

Matrix4x4&	Matrix4x4::operator += (const Matrix4x4& mat)
{
	for( u8 i = 0; i < 16; i++ )
	{
		m[i / 4][i % 4] += mat.m[i / 4][i % 4];
	}
	return *this;
}

//-----------------------------------------------------------------------------

Matrix4x4&	Matrix4x4::operator -= (const Matrix4x4& mat)
{
	for( u8 i = 0; i < 16; i++ )
	{
		m[i / 4][i % 4] -= mat.m[i / 4][i % 4];
	}
	return *this;
}

//-----------------------------------------------------------------------------

Matrix4x4&	Matrix4x4::operator *= (const Matrix4x4& mat)
{
    Matrix4x4 tmp = *this;
	for (u8 i = 0; i < 4; i++ )
	{
		tmp.m[i][0] =	(m[i][0] * mat.m[0][0]) +
					(m[i][1] * mat.m[1][0]) +
					(m[i][2] * mat.m[2][0]) +
					(m[i][3] * mat.m[3][0]) ;

		tmp.m[i][1] =	(m[i][0] * mat.m[0][1]) + 
					(m[i][1] * mat.m[1][1]) +
					(m[i][2] * mat.m[2][1]) +
					(m[i][3] * mat.m[3][1]) ;

		tmp.m[i][2] =	(m[i][0] * mat.m[0][2]) + 
					(m[i][1] * mat.m[1][2]) +
					(m[i][2] * mat.m[2][2]) +
					(m[i][3] * mat.m[3][2]) ;

		tmp.m[i][3] =	(m[i][0] * mat.m[0][3]) + 
					(m[i][1] * mat.m[1][3]) +
					(m[i][2] * mat.m[2][3]) +
					(m[i][3] * mat.m[3][3]) ;
	}
    *this = tmp;
	return *this;
}

//-----------------------------------------------------------------------------

Matrix4x4	Matrix4x4::GetGLMatrix()
{
	//Matrix4x4 tmp(*this);

	//tmp.m[0][1] = m[1][0];
	//tmp.m[0][2] = m[2][0];

	//tmp.m[1][0] = m[0][1];
	//tmp.m[2][0] = m[0][2];

	//tmp.m[2][1] = m[1][2];
	//tmp.m[1][2] = m[2][1];

	return GetTranspose();
}

//-----------------------------------------------------------------------------


Matrix4x4&	Matrix4x4::operator = (const f32& val)
{
	memset( m, val, sizeof( f32 ) * 16 );
	return *this;
}

//-----------------------------------------------------------------------------

Matrix4x4&	Matrix4x4::operator += (const f32& val)
{
	for( u8 i = 0; i < 16; i++ )
	{
		m[i / 4][i % 4] += val;
	}
	return *this;
}

//-----------------------------------------------------------------------------

Matrix4x4&	Matrix4x4::operator -= (const f32& val)
{
	for( u8 i = 0; i < 16; i++ )
	{
		m[i / 4][i % 4] -= val;
	}
	return *this;
}

//-----------------------------------------------------------------------------

Matrix4x4&	Matrix4x4::operator *= (const f32& val)
{
	for( u8 i = 0; i < 16; i++ )
	{
		m[i / 4][i % 4] *= val;
	}
	return *this;
}

//-----------------------------------------------------------------------------

Matrix4x4&	Matrix4x4::operator /= (const f32& val)
{
	for( u8 i = 0; i < 16; i++ )
	{
		m[i / 4][i % 4] /= val;
	}
	return *this;
}

//-----------------------------------------------------------------------------

const Matrix4x4	Matrix4x4::operator + (const Matrix4x4& mat) const
{
	return Matrix4x4(*this) += mat;
}

//-----------------------------------------------------------------------------

const Matrix4x4	Matrix4x4::operator - (const Matrix4x4& mat) const
{
	return Matrix4x4(*this) -= mat;
}

//-----------------------------------------------------------------------------

const Matrix4x4	Matrix4x4::operator * (const Matrix4x4& mat) const
{
	return Matrix4x4(*this) *= mat;
}

//-----------------------------------------------------------------------------


const Matrix4x4	Matrix4x4::operator + (const f32& val) const
{
	return Matrix4x4(*this) += val;
}

//-----------------------------------------------------------------------------

const Matrix4x4	Matrix4x4::operator - (const f32& val) const
{
	return Matrix4x4(*this) -= val;
}

//-----------------------------------------------------------------------------

const Matrix4x4	Matrix4x4::operator * (const f32& val) const
{
	return Matrix4x4(*this) *= val;
}

//-----------------------------------------------------------------------------

const Matrix4x4	Matrix4x4::operator / (const f32& val) const
{
	return Matrix4x4(*this) /= val;
}

//-----------------------------------------------------------------------------
///////////////////////////////////////////////////////////////////////////////
